#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    Gyro,           sensorGyro)
#pragma config(Sensor, in2,    ArmPot,         sensorPotentiometer)
#pragma config(Sensor, in3,    FlipperPot,     sensorPotentiometer)
#pragma config(Sensor, in4,    Auton_Pot,      sensorPotentiometer)
#pragma config(Sensor, dgtl1,  JawChanger,     sensorTouch)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           RightBaseBack, tmotorVex393HighSpeed, openLoop, reversed, encoder, encoderPort, I2C_1, 1000)
#pragma config(Motor,  port2,           LeftBaseFront, tmotorVex393HighSpeed, openLoop)
#pragma config(Motor,  port3,           RightArm,      tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port4,           LeftArm,       tmotorVex393, openLoop)
#pragma config(Motor,  port6,           FlipperMotorLeft, tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port7,           FlipperMotorRight, tmotorVex269, openLoop)
#pragma config(Motor,  port8,           LeftBaseBack,  tmotorVex393, openLoop, encoder, encoderPort, I2C_2, 1000)
#pragma config(Motor,  port9,           Convayer,      tmotorVex393HighSpeed, openLoop)
#pragma config(Motor,  port10,          RightBaseFront, tmotorVex393HighSpeed, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

// Pot Values
int Trough = 2222;
int HighGoal = 2222; // "
int HighLimit = 5000; // Placeholder for the moment...
int LowLimit = 1080; // "
int Range0 = 5000;
int Range1 = 2000;
int Range2 = 1000;
int Range3 = 0;

#define ArmsFloor 1060
#define FlipperToFloor 3250 //2750 //2600//2500//2600//2560
#define FlipperScore 833
#define FlipperScoreHeavy -100
#define FlipperUp 100
#define FlipperHoldHeavy 25
#define FlipperHoldLight 10
#define FlipperFast 127
#define FlipperSlow 60
#define FlipperMedium 90

void flippergodown1()
{
	while (SensorValue [FlipperPot] > FlipperToFloor)
	{
		motor(FlipperMotorLeft) = -63;
		motor(FlipperMotorRight) = -63;
	}
	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;
}

void flipperup1()
{
	//jaws will be open
	//aFTER 45 DEGREES CLOSE JAWS
	while (SensorValue [FlipperPot] < 3641)
	{
		motor(FlipperMotorLeft) = 127;
		motor(FlipperMotorRight) = 127;
		if (SensorValue [FlipperPot] > 3641)
		{
			break;
		}
	}
	wait1Msec(1000);
	motor(FlipperMotorLeft) = 10;
	motor(FlipperMotorRight) = 10;

}

void flippertotrough1()
{
	// first reead the pot on flipper into a local variable
	// keep movimg the flliper down until it reaCHES  the "trough" pot setting
	// once it reaches the trough setting - quit moving the flipper
	while (SensorValue[FlipperPot] > FlipperScore)
	{
		motor(FlipperMotorRight) = -83;
		motor(FlipperMotorLeft) = -83;
	}
	motor(FlipperMotorLeft) = 0;
	motor(FlipperMotorRight) = 0;
}

task TASKflipperup()
{
	//jaws will be open
	while (SensorValue [FlipperPot] < 3000)
	{
		motor(FlipperMotorLeft) = 127;
		motor(FlipperMotorRight) = 127;
		wait1Msec(10);
		motor(FlipperMotorLeft) = 10;
		motor(FlipperMotorRight) = 10;
		wait1Msec(10);
	}
	motor(FlipperMotorLeft) = 10;
	motor(FlipperMotorRight) = 10;
}

task TASKflippertofloorandhold
{
	// raising up from trough position scored
	while (SensorValue [FlipperPot] < FlipperToFloor)
	{
		motor(FlipperMotorLeft) = 127;
		motor(FlipperMotorRight) = 127;
	}
	motor(FlipperMotorLeft) = 10;
	motor(FlipperMotorRight) = 10;
}

void ClearEncoders()
{
	nMotorEncoder[RightBaseBack] = 0;
	nMotorEncoder[RightBaseFront] = 0;
	nMotorEncoder[LeftBaseBack] = 0;
	nMotorEncoder[LeftBaseFront] = 0;
}

void MoveFullSpeed(bool bForwards, int iPosition)
{
	ClearEncoders();
	while (abs(nMotorEncoder(RightBaseBack)) < iPosition)
	{
		if (bForwards)
		{
			motor[RightBaseFront] = 127;
			motor[RightBaseBack] = 127;
			motor[LeftBaseFront] = 127;
			motor[LeftBaseBack] = 127;
		}
		else
		{
			motor[RightBaseFront] = -127;
			motor[RightBaseBack] = -127;
			motor[LeftBaseFront] = -127;
			motor[LeftBaseBack] = -127;
		}
	}
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;
}

void TheNewcoreFive()
{
	flippergodown1();

	//drive forward to five stack
	MoveFullSpeed(true, 900); //1100

	StartTask(TASKflipperup);

	while(SensorValue(ArmPot) < Trough)
	{
		motor(RightArm) = 127;
		motor(LeftArm) = 127;
	}
	motor(RightArm) = 10;
	motor(LeftArm) = 10;

	// drive forward
	MoveFullSpeed(true, 185);//165

	StopTask(TASKflipperup);

	flippertotrough1();
	wait1Msec(500);

	// drive back
	MoveFullSpeed(false, 500);

	// turn towards yellow sack
	motor[RightBaseFront] = -127;
	motor[RightBaseBack] = -127;
	motor[LeftBaseFront] = 127;
	motor[LeftBaseBack] = 127;
	wait1Msec(250);
	motor[RightBaseFront] = 0;
	motor[RightBaseBack] = 0;
	motor[LeftBaseFront] = 0;
	motor[LeftBaseBack] = 0;

	StartTask(TASKflippertofloorandhold);

	while(SensorValue(ArmPot) > ArmsFloor)
	{
		motor(RightArm) = -127;
		motor(LeftArm) = -127;
	}
	motor(RightArm) = 0;
	motor(LeftArm) = 0;

	StopTask(TASKflippertofloorandhold);

	// get yellow sack
	//motor[RightBaseFront] = 127;
	//motor[RightBaseBack] = 127;
	//motor[LeftBaseFront] = 127;
	//motor[LeftBaseBack] = 127;
	//wait1Msec(1000);
	//motor[RightBaseFront] = 0;
	//motor[RightBaseBack] = 0;
	//motor[LeftBaseFront] = 0;
	//motor[LeftBaseBack] = 0;

}

void WaitForLCD()
{

	while(true)
	{
		// we will be in this loop until the user pushes a button
		// check if button has been pressed if so break out of loop
		if (nLCDButtons != 0)
		{
			// somebody pressed a button - break out of this loop and continue on...
			break;
		}
	}
}

task main()
{
	WaitForLCD();
	TheNewcoreFive();
}
